#ifndef OBTEIN_MINERAL_TAASK_H
#define OBTEIN_MINERAL_TAASK_H
#include "main.h"
#include "CAN_receive.h"
#include "pid.h"
#include "remote_control.h"
#include "user_lib.h"
#include "ins_task.h"
#include "power_control.h"
/**************************操作按键********************************/
#define  CLAMP_UP_KEY            KEY_PRESSED_OFFSET_R    //夹爪向下旋转
#define  CLAMP_DOWN_KEY          KEY_PRESSED_OFFSET_F    //夹具向下旋转

#define  FRICTION_CONTROL_KEY    KEY_PRESSED_OFFSET_SHIFT  
#define  FRICTIONY_KEY           KEY_PRESSED_OFFSET_Q    //Shift+,,,,摩擦轮四个方向调整方向
#define  FRICTION_Y_KEY          KEY_PRESSED_OFFSET_E
#define  FRICTIONX_KEY           KEY_PRESSED_OFFSET_Z
#define  FRICTION_X_KEY          KEY_PRESSED_OFFSET_C

#define  GO_KEY                  KEY_PRESSED_OFFSET_R  //升
#define  DOWN_KEY                KEY_PRESSED_OFFSET_F    //降

#define  FRICTIONX_KEY           KEY_PRESSED_OFFSET_Z
#define  FRICTION_X_KEY          KEY_PRESSED_OFFSET_C

#define  SAVE_OBTEIN_KEY         KEY_PRESSED_OFFSET_V   //存储/取出矿石

#define  CLAMPING_CONTROL_KEY    KEY_PRESSED_OFFSET_CTRL //Ctr+Z,X,C分别抓取对应的三块矿石
#define  CLAMPING_ONE_KEY        KEY_PRESSED_OFFSET_Z
#define  CLAMPING_TWO_KEY        KEY_PRESSED_OFFSET_X
#define  CLAMPING_THREE_KEY      KEY_PRESSED_OFFSET_C

#define  RESET_KEY               KEY_PRESSED_OFFSET_G    //回到夹取初始状态

/**************************操作按键*******************************/
//夹取初始化任务延时
#define obtein_mineral_task_INIT_TIME  200  
//夹取任务一阶滤波系数
#define engineering_CONTROL_TIME    0.002f 
//横向臂最大速度（只按方向键）
#define NORMAL_MAX_translation_SPEED   1.0f
//夹具旋转最大速度（只按方向键）
#define NORMAL_MAX_turn_SPEED       0.01f
//摩擦轮最大速度（只按方向键）
#define NORMAL_MAX_friction_SPEED  100
//工程任务状态
#define engineer_mode_channel1 1
#define engineer_mode_channel0 0
//横向臂对应矿石的距离
#define Clamping_1t2    -2250     //中间到左边矿石的距离 （电机旋转的角度 单位（°））
#define Clamping_1t3     2250     //中间到右边矿石的距离 （电机旋转的角度 单位（°））

#define clamping_set_angle     170   //自动夹取的目标角度（°）
#define clamping_middle_angle  80    //自动夹取的中间角度（°）
#define clamping_init_angle    10
//计算电机速度系数
#define MOTOR_SPEED_TO_enginner_clamp         0.25f
#define MOTOR_SPEED_TO_enginner_translation   0.25f
//校准步骤
#define engineer_CALI_START_STEP     1
#define clamp_CALL_MIN_STEP          1
#define clamp_CALL_MAX_STEP          2
#define engineer_CALL_END_STEP		 3
//校准电流
#define engineer_cali_set       1000
#define engineer_cali_time      1000

//摩擦轮舵机角度旋转
#define Friction_Servo_angle    30 

//电机反馈码盘值范围
#define HALF_ECD_RANGE          4096          //电机编码值的一半
#define ECD_RANGE               8191          //电机转子旋转一圈的编码值
//电机rmp 变化成 旋转速度的比例
#define MOTOR_RPM_TO_SPEED              0.003f
#define MOTOR_ECD_TO_ANGLE2006          0.00115f    //2006电机欧拉角转速比
#define MOTOR_ECD_TO_ANGLE3508          0.0023f     //3508电机欧拉角转速比
#define FULL_COUNT                      9.5

#define GO_DOWN_MAX_HIGH                100    //只按按键取矿石的最大高度
#define EXSCHANGE_MAX_HIGH              120    //兑换矿石抬升的高度
#define DO_DOWN_ADD_HIGH                10     //取矿石突然抬升给的高度
//鼠标xyz比例系数
#define mouse_z			   0.00005f
#define mouse_go_down      0.05f

//最大速度限制
#define MAX_CLAMP_SPEED        1
#define MAX_TRANSLATION_SPPED  1
#define MAX_FRC_SPEED		   1

//夹具气缸工作设定
#define Cylinder_Clamp_set()		   power_open(1)
#define Cylinder_Clamp_reset()		   power_off(1)
#define Cylinder_Stretch_set()         power_open(2)
#define Cylinder_Stretch_reset()       power_off(2)

#define read_translation__L_limit()    HAL_GPIO_ReadPin(Q1_GPIO_Port, Q1_Pin)

#define read_Cylinder_Stretch_reset()    HAL_GPIO_ReadPin(Q2_GPIO_Port, Q2_Pin)     //霍尔传感器 

#define translation_cail_limit()	   HAL_GPIO_ReadPin(P1_GPIO_Port, P1_Pin)     //横移中间限位

#define read_Clamping_limit()		   HAL_GPIO_ReadPin(M2_GPIO_Port, M2_Pin)     //夹具触发限位

#define read_friction_limit()		   HAL_GPIO_ReadPin(J1_GPIO_Port, J1_Pin)     //存储矿石限位

#define clamp_cali_limit()             HAL_GPIO_ReadPin(M1_GPIO_Port, M1_Pin)     //夹具校准限位

//m3508转化成底盘速度(m/s)的比例，
#define M3508_MOTOR_RPM_TO_VECTOR           0.0004f
#define CHASSIS_MOTOR_RPM_TO_VECTOR_SEN     M3508_MOTOR_RPM_TO_VECTOR

//底盘任务控制频率
#define CHASSIS_CONTROL_FREQUENCE   500.0f  

//横向臂 角度环 PID参数以及 PID最大输出，积分输出
#define translation_ENCODE_RELATIVE_PID_KP 100.0f      //0.3f
#define translation_ENCODE_RELATIVE_PID_KI 0.0f       //0.0f
#define translation_ENCODE_RELATIVE_PID_KD 0.6f       //0.15f
#define translation_ENCODE_RELATIVE_PID_MAX_OUT 4000.0f //70f
#define translation_ENCODE_RELATIVE_PID_MAX_IOUT 50.0f  //0.0f

//横向臂3508 速度环pid
#define M3508_translation_MOTOR_SPEED_PID_KP 18.0f
#define M3508_translation_MOTOR_SPEED_PID_KI 0.0f
#define M3508_translation_MOTOR_SPEED_PID_KD 0.8f
#define M3508_translation_MOTOR_SPEED_MAX_OUT  3000.0f
#define M3508_translation_MOTOR_SPEED_MAX_IOUT 50.0f

//夹具 角度环 PID参数以及 PID最大输出，积分输出
#define clamp_ENCODE_RELATIVE_PID_KP 50.0f  
#define clamp_ENCODE_RELATIVE_PID_KI 0.0f    
#define clamp_ENCODE_RELATIVE_PID_KD 0.5f  
#define clamp_ENCODE_RELATIVE_PID_MAX_OUT 6000.0f 
#define clamp_ENCODE_RELATIVE_PID_MAX_IOUT 80.0f 

//夹具旋转3508 速度环PID
#define M3508_Clamp_turn_MOTOR_SPEED_PID_KP 12.0f    
#define M3508_Clamp_turn_MOTOR_SPEED_PID_KI 0.0f     
#define M3508_Clamp_turn_MOTOR_SPEED_PID_KD 0.5f      
#define M3508_Clamp_turn_MOTOR_SPEED_PID_MAX_OUT 4500.0f
#define M3508_Clamp_turn_MOTOR_SPEED_PID_MAX_IOUT 80.0f

//摩擦轮轮电机PID
#define M2006_fric_PID_KP        500.0f     
#define M2006_fric_PID_KI        0.2f
#define M2006_fric_PID_KD        0.6f
#define M2006_fric_PID_MAX_OUT   1600.0f
#define M2006_fric_PID_MAX_IOUT  20.0f

//滤波
#define engineering_translation_NUM 0.3333f
#define engineering_Clamp_turn_NUM  0.0167f
#define engineering_friction_NUM    0.0667f

/*****夹取状态机*****/
typedef enum          
{
	engineering_Clamping_init =  0,                     //夹具 初始状态
	engineering_Clamping_ready,                         //夹具 准备夹取
	engineering_Clamping,                               //夹具 夹取
	engineering_Clamp_check,                            //夹具 检查
	engineering_No_Clamp,                               //夹具 松开

} engineer_obtein_state_e;

typedef enum 
{
	engineer_motor_raw = 0,
	engineer_motor_encode
}engineer_motor_mode_e;

typedef enum
{
  Servo_reset,
  Servo_set,
}engineer_Servos_e;

//对应3个矿石
typedef enum
{
	 Clamping_1 = 1,
	 Clamping_2 = 2,
	 Clamping_3 = 3,
}Clamping_e;

typedef struct
{
	const motor_measure_t* motor_measure;
	fp32 angle;
	fp32 accel;
	fp32 speed;
	fp32 speed_set;
	int16_t give_current;
} engineering_obtein_motor_t;

typedef struct
{
	fp32 clamp_max_angle;								//夹具最大角度
	fp32 clamp_min_angle;								//夹具最小角度
	uint16_t clamp_min_ced;                             //两个电机的ecd不一定相等，目前只校准电机2
	uint8_t step;
} engineer_step_cali_t;

typedef struct  
{
    uint8_t go_down_add_high_flag;                                 //有必要再次抬升标志位
	engineer_Servos_e Servo_state;
	const RC_ctrl_t* engineering_obtein_RC;
	const imu_t* engineering_INS_angle;
	const can_receive_t* can_chassis_A;							   //获取底盘A板数据
	Clamping_e Clamping_obtein;                                        //获取那一个矿石
	Clamping_e last_Clamping_obtein;
	engineer_obtein_state_e engineer_obtein_state;
	engineer_obtein_state_e last_engineer_obtein_state;
	engineer_motor_mode_e engineer_motor_mode;
	engineer_motor_mode_e last_engineer_motor_mode;

	engineer_step_cali_t engineer_cali;


  bool_t Chassis_Directions;
	engineering_obtein_motor_t engineer_translation_motor_data;       //横向臂
	engineering_obtein_motor_t engineer_Clamp_turn_motor_data[2];     //夹具旋转
	engineering_obtein_motor_t engineer_frction_motor_data[4];     //摩擦轮

	can_transmit_t transmit_chassis;                              //发送数据到底盘A板
	//获取升降电机和摩擦轮电机

	pid_type_def translation_motor_mileage_pid;
	pid_type_def engineer_translation_motor_pid;
	
	pid_type_def engineer_friction_motor_pid[4];

	pid_type_def clamp_relative_angle_pid[2];
	pid_type_def engineer_Clamp_turn_motor_pid[2];

	//fp32 go_down_speed_set;
	fp32 translation_speed_set;                      //对应每个机构的运动
	fp32 Clamp_turn_speed_set;
	fp32 friction_x_speed_set;
	fp32 friction_y_speed_set;

    fp32 Clamp_Init_Angle;                            //夹取电机初始化角度
	fp32 translation_speed;
	fp32 Clamp_turn_speed;
	fp32 friction_speed;

	uint16_t clamp_min_ced;							 //电机上电初始化角度，上电时候记录一次，目前记录2号电机
	fp32 clamp_max_angle;
    fp32 clamp_min_angle;
	//int16_t clamp_turn_num[2];                           //夹具编码器转动圈数  在CAN中断里面
	fp32 clamp_turn_set_angle;							   //下步期望角度
	fp32 clamp_turn_last_angle;							   //现在角度
	fp32 translation_set_mileage;                          //下步期望里程计
	fp32 translation_last_mileage;                         //现在里程计

	first_order_filter_type_t engineer_cmd_slow_set_translation;		 //使用一阶低通滤波减缓设定值
	first_order_filter_type_t engineer_cmd_slow_set_clamp_turn;			 //使用一阶低通滤波减缓设定值
	first_order_filter_type_t engineer_cmd_slow_set_friction_x;          //使用一阶低通滤波减缓设定值
	first_order_filter_type_t engineer_cmd_slow_set_friction_y;          //使用一阶低通滤波减缓设定值
          
	fp32 Clamp_turn_motor_max_speed;                      //夹具旋转最大速度
	fp32 Clamp_turn_motor_min_speed;                      //夹具旋转最大速度
	fp32 translation_motor_max_speed;                     //横向臂最大速度
	fp32 translation_motor_min_speed;                     //横向臂最大速度					  
	fp32 go_down_motor_max_speed;
	fp32 go_down_motor_min_speed;
	fp32 friction_motor_max_speed;
	fp32 friction_motor_min_speed;

	uint8_t Cylinder_Stretch_flg;
	uint8_t Cylinder_Clamp_flg;

	uint8_t fric_deposit;                    //存矿标志
	uint8_t last_fric_deposit;
	uint8_t fric_exchange;
	ramp_function_source_t ramp;               //斜波数据

}engineering_obtein_move_t;


extern engineering_obtein_move_t engineering_obtein_move;
/**
  * @brief          夹取矿石任务，由main函数创建
  * @param[in]      pvParameters: 空
  * @retval         none
  */
extern void obtein_mineral_task(void const* pvParameters);
/**
  * @brief          设置获取矿石状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
extern void engineering_obtein_set_state(engineering_obtein_move_t* engineer_set_state);
/**
  * @brief          设置状态改变
  * @param[in]      pvParameters: 空
  * @retval         none
  */
extern void engineering_obtein_set_change(fp32* translation,fp32* clamp, engineering_obtein_move_t* engineer_set_change);
/**
  * @brief          兑换矿石状态改变
  * @param[in]      pvParameters: 空
  * @retval         none
  */
 extern void engineering_exchange_set_change(fp32* clamp,engineering_obtein_move_t* engineering_exchange_change);
/**
  * @brief          设置控制
  * @param[in]      pvParameters: 空
  * @retval         none
  */
extern void engineering_obtein_set_control(fp32* clamp,fp32* frc_x,fp32* frc_y, engineering_obtein_move_t* engineer_set_control);
/**
  * @brief          夹具校准设置，将校准的 中值以及最小最大机械相对角度
  * @param[in]      yaw_offse:yaw 中值
  * @param[in]      pitch_offset:pitch 中值
  * @param[in]      max_yaw:pitch 最大相对角度
  * @param[in]      min_yaw:pitch 最小相对角度
  * @retval         返回空
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
extern void set_cali_clamp_hook(const uint16_t clamp_ecd2, const fp32 max_clamp, const fp32 min_clamp);
/**
  * @brief          夹具校准计算，将校准记录的中值,最大 最小值返回
  * @param[out]     min_ced 最小ecd
  * @param[out]     max_clamp 最大相对角度 指针
  * @param[out]     min_clamp 最小相对角度 指针
  * @retval         返回1 代表成功校准完毕， 返回0 代表未校准完
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
extern bool_t cmd_cali_clamp_hook(uint16_t* min_ced, fp32* max_clamp, fp32* min_clamp);

#endif

